
#include <level/player/player.hpp>
#include <physics/collision_message/collision_message.hpp>
#include <boost/bind.hpp>

#include <input/InputSingleton.h>
#include <input/actions/actions.hpp>
#include <nmn/create_string.hpp>
#include <window/window.hpp>

#include <level/messages/kill_object.hpp>
#include <level/messages/level_over.hpp>

b2Vec2 const aabb_half_size( 2.325f, 2.4f );
b2Vec2 const sprite_half_size( 4.8f, 4.8f );
float const kill_speed_factor = 0.01f;
float const time_off_floor_before_fall = 0.1f;
float const jump_power = 1.0f;
float const jump_movement_power = 60.0f;
float const max_jump_charge_time = 0.15f;
float const max_motor_speed = 20.0f * b2_pi;
float const hacky_jump_timeout = 0.01f;

class player_user_data : public base_user_data
{
public:

	player_user_data( body_weak_ptr body )
		:	base_user_data( body )
	{}

	void on_contact( b2Contact * /*contact*/, base_user_data * /*self_user_data*/, base_user_data * /*other_user_data*/ ) { /*OutputDebugStringA( "Collision!\n" );*/ }
};

class aabb_query_callback : public b2QueryCallback
{
public:

	aabb_query_callback( body_ptr player_a, body_ptr player_b )
		:		hit_anything(false)
			,	a(player_a)
			,	b(player_b)
	{}

	bool ReportFixture( b2Fixture * fixture )
	{
		if ( fixture->GetBody( ) != a.get( ) && fixture->GetBody( ) != b.get( ) )
		{
			for ( b2Fixture * a_fixture = a->GetFixtureList( ); a_fixture != nullptr; a_fixture = a_fixture->GetNext( ) )
			{
				if ( touching( a_fixture, fixture ) )
				{
					hit_anything = true;
					return false;
				}
			}

			for ( b2Fixture * b_fixture = b->GetFixtureList( ); b_fixture != nullptr; b_fixture = b_fixture->GetNext( ) )
			{
				if ( touching( b_fixture, fixture ) )
				{
					hit_anything = true;
					return false;
				}
			}
		}

		return true;
	}

	bool touching( b2Fixture * a_fixture, b2Fixture * b_fixture )
	{
		if ( a_fixture->IsSensor( ) || b_fixture->IsSensor( ) )
		{
			return false;
		}

		b2DistanceOutput distance_output; memset( &distance_output, 0, sizeof( b2DistanceOutput ) );

		b2DistanceInput distance_input; memset( &distance_input, 0, sizeof( b2DistanceInput ) );
		distance_input.proxyA.Set( a_fixture->GetShape( ) );
		distance_input.proxyB.Set( b_fixture->GetShape( ) );
		distance_input.transformA = a_fixture->GetBody( )->GetTransform( );
		distance_input.transformB = b_fixture->GetBody( )->GetTransform( );
		distance_input.useRadii = false;

		b2SimplexCache simplex_cache; memset( &simplex_cache, 0, sizeof( b2SimplexCache ) );

		b2Distance( &distance_output, &simplex_cache, &distance_input );
		//OutputDebugStringA( nmn::create_string( "distance: %1%\n", distance_output.distance ).c_str( ) );

		return distance_output.distance <= 0.025f;
	}

	bool hit_anything;
	body_ptr a;
	body_ptr b;

	b2BlockAllocator m_allocator;
};

player::player( )
	:		m_things(0)
		,	m_direction(player_direction::right)
		,	m_state(player_state::idle)
		,	m_jump_held_for( 0.0f )
		,	m_falling( false )
		,	m_has_been_dead_for( 0.0f )
		,	m_time( 0.0f )
		,	m_time_last_touched_floor( -time_off_floor_before_fall )
		,	m_hacky_jump_timer(0.0f)

		,	m_pickup_sound( *g_cache.load_buffer( "./resources/audio/pickup_thing.wav" ) )
		,	m_jump_sound( *g_cache.load_buffer( "./resources/audio/jump.wav" ) )
		,	m_landed_sound( *g_cache.load_buffer( "./resources/audio/landed.wav" ) )
		,	m_death_sound( *g_cache.load_buffer( "./resources/audio/death.wav" ) )
{
	m_state_animations[player_state::idle][player_direction::left].reset( new animation( "./resources/images/level/player/idle_left.anim" ) );
	m_state_animations[player_state::idle][player_direction::right].reset( new animation( "./resources/images/level/player/idle_right.anim" ) );

	m_state_animations[player_state::running][player_direction::left].reset( new animation( "./resources/images/player/running_left.anim" ) );
	m_state_animations[player_state::running][player_direction::right].reset( new animation( "./resources/images/player/running_right.anim" ) );

	m_state_animations[player_state::jumping][player_direction::left].reset( new animation( "./resources/images/player/jumping_left.anim" ) );
	m_state_animations[player_state::jumping][player_direction::right].reset( new animation( "./resources/images/player/jumping_right.anim" ) );

	m_state_animations[player_state::dying][player_direction::left].reset( new animation( "./resources/images/level/player/dying.anim" ) );
	m_state_animations[player_state::dying][player_direction::right] = m_state_animations[player_state::dying][player_direction::left];

	m_messenger_connection = g_messenger.add_listener( collision_message::name( ), boost::bind( &player::message_callback, this, _1, _2 ) );

	g_user_input.assignActionToKey( actions::run_left, sf::Key::Left );
	g_user_input.assignActionToKey( actions::run_right, sf::Key::Right );
	g_user_input.assignActionToKey( actions::jump, sf::Key::Space );
}

void player::step( float delta_time )
{
	m_time += delta_time;

	switch ( m_state )
	{
	case player_state::idle:
		{
			if ( is_in_air( ) || ( m_hacky_jump_timer >= hacky_jump_timeout && g_user_input.getActionInput( actions::jump ).status == keyPressed ) )
			{
				switch_state( player_state::jumping );
			}
			else if ( g_user_input.getActionInput( actions::run_left ).down( ) || g_user_input.getActionInput( actions::run_right ).down( ) )
			{
				switch_state( player_state::running );
			}
		}
		break;

	case player_state::running:
		{
			if ( is_in_air( ) || ( m_hacky_jump_timer >= hacky_jump_timeout && g_user_input.getActionInput( actions::jump ).status == keyPressed ) )
			{
				switch_state( player_state::jumping );
			}
			else if ( g_user_input.getActionInput( actions::run_left ).down( ) || g_user_input.getActionInput( actions::run_right ).down( ) )
			{
				float speed_change = 0.0f;

				if ( g_user_input.getActionInput( actions::run_left ).down( ) )
				{
					speed_change -= b2_pi;
				}

				if ( g_user_input.getActionInput( actions::run_right ).down( ) )
				{
					speed_change += b2_pi;
				}

				m_revolute->SetMotorSpeed( m_revolute->GetMotorSpeed( ) + speed_change );

				if ( std::abs( speed_change ) >= 0.001f )
				{
					float change_speed_sign = b2Sign( speed_change );
					float current_speed_sign = b2Sign( m_revolute->GetMotorSpeed( ) );

					//	We're trying to change direction. Drastically reduce speed to help the player out.
					if ( std::abs( change_speed_sign - current_speed_sign ) >= 0.001f )
					{
						m_revolute->SetMotorSpeed( m_revolute->GetMotorSpeed( ) * kill_speed_factor );
					}
				}
			}
			else
			{
				switch_state( player_state::idle );
			}
		}
		break;

	case player_state::jumping:
		{
			if ( !m_falling )
			{
				m_jump_held_for += delta_time;

				float const total_mass = m_lower_body->GetMass( ) + m_upper_body->GetMass( );
				if ( g_user_input.getActionInput( actions::jump ).down( ) && m_jump_held_for < max_jump_charge_time )
				{
					//	apply upwards force
					m_upper_body->ApplyLinearImpulse( total_mass * b2Vec2( 0.0f, -jump_power ), m_upper_body->GetPosition( ) );
				}

				if ( g_user_input.getActionInput( actions::run_left ).down( ) )
				{
					m_upper_body->ApplyForce( total_mass * b2Vec2( -jump_movement_power, 0.0f ), m_upper_body->GetPosition( ) );
				}

				if ( g_user_input.getActionInput( actions::run_right ).down( ) )
				{
					m_upper_body->ApplyForce( total_mass * b2Vec2( jump_movement_power, 0.0f ), m_upper_body->GetPosition( ) );
				}
			}

			if ( !is_in_air( ) )
			{
				m_landed_sound.Play( );
				switch_state( player_state::idle );
				break;
			}
		}
		break;

	case player_state::dying:
		{
			m_has_been_dead_for += delta_time;
			if ( m_has_been_dead_for >= 1.0f )
			{
				g_messenger.push( level_over_message::name( ), message_data_ptr( new level_over_message( false ) ) );
			}
		}
		break;
	};

	if ( g_user_input.getActionInput( actions::run_left ).down( ) )
	{
		switch_direction( player_direction::left );
	}
	else if ( g_user_input.getActionInput( actions::run_right ).down( ) )
	{
		switch_direction( player_direction::right );
	}

	if ( m_state != player_state::running )
	{
		m_revolute->SetMotorSpeed( m_revolute->GetMotorSpeed( ) * kill_speed_factor );
	}

	m_revolute->SetMotorSpeed( b2Sign( m_revolute->GetMotorSpeed( ) ) * std::min( max_motor_speed, std::abs( m_revolute->GetMotorSpeed( ) ) ) );

	m_state_animations[m_state][m_direction]->update( delta_time );

	m_hacky_jump_timer += delta_time;

	//OutputDebugStringA( "=====================\n" );
	//OutputDebugStringA( nmn::create_string( "Upper Linear Velocity: { %1%, %2% }\n", m_upper_body->GetLinearVelocity( ).x, m_upper_body->GetLinearVelocity( ).y ).c_str( ) );
	//OutputDebugStringA( nmn::create_string( "Lower Linear Velocity: { %1%, %2% }\n", m_lower_body->GetLinearVelocity( ).x, m_lower_body->GetLinearVelocity( ).y ).c_str( ) );
}

void player::draw( )
{
	m_state_animations[m_state][m_direction]->move_to( g_scale_b2_position * m_upper_body->GetPosition( ) );
	m_state_animations[m_state][m_direction]->draw( );

	//static std::string state_names[player_state::number_of] =
	//{
	//	"idle",
	//	"running",
	//	"jumping",
	//	"dying"
	//};

	//sf::String string( nmn::create_string( "%1%\nfalling: %2%\nRPM: %3%", state_names[m_state], m_falling, m_revolute->GetMotorSpeed( ) / b2_pi ) );
	//string.SetColor( sf::Color::Red );
	//g_window->Draw( string );

	//b2Vec2 const fudge( 0.2f, 0.2f );

	//b2AABB aabb;
	//aabb.lowerBound = m_upper_body->GetPosition( ) + ( sprite_half_size - aabb_half_size ) + fudge;
	//aabb.upperBound = m_upper_body->GetPosition( ) + ( sprite_half_size + aabb_half_size ) - fudge;
	//aabb.upperBound.y += aabb_half_size.x;

	//debug_draw drawer;
	////drawer.DrawAABB( &aabb, b2Color(1.0f, 0.0f, 0.0f ) );
	//drawer.DrawSegment( m_lower_body->GetPosition( ),
	//					m_lower_body->GetPosition( ) + m_lower_body->GetLinearVelocity( ),
	//					b2Color( 1.0f, 0.0f, 0.0f ) );

	//drawer.DrawSegment( m_upper_body->GetPosition( ) + b2Vec2( 0.0f, 0.1f ),
	//					m_upper_body->GetPosition( ) + m_upper_body->GetLinearVelocity( ) + b2Vec2( 0.0f, 0.1f ),
	//					b2Color( 0.0f, 0.0f, 1.0f ) );
}

bool player::can_add_to_world( world_ptr world, b2Vec2 const & position ) const
{
	aabb_query_callback query( m_upper_body, m_lower_body );

	b2Vec2 const fudge( 0.2f, 0.2f );

	b2AABB aabb;
	aabb.lowerBound = position + ( sprite_half_size - aabb_half_size ) + fudge;
	aabb.upperBound = position + ( sprite_half_size + aabb_half_size ) - fudge;
	aabb.upperBound.y += aabb_half_size.x;

	world->QueryAABB( &query, aabb );

	return !query.hit_anything;
}

void player::add_to_world( world_ptr world, b2Vec2 const & position )
{
	//OutputDebugStringA( "Creating Player\n" );

	m_hacky_jump_timer = 0.0f;
	m_time_last_touched_floor = m_time;

	if ( m_upper_body != nullptr && m_upper_body->GetWorld( ) == world.get( ) )
	{
		return;
	}

	bool relocating = m_upper_body != nullptr;

	//m_revolute.reset( );
	//m_upper_body.reset( );
	//m_lower_body.reset( );

	b2BodyDef upper_body_definition;
	b2BodyDef lower_body_definition;
	b2RevoluteJointDef revolute_joint_definition;
	float motor_speed = m_revolute ? m_revolute->GetMotorSpeed( ) : 0.0f;
	m_revolute.reset( );

	b2Vec2 lower_body_linear_velocity;
	lower_body_linear_velocity.SetZero( );
	float32 lower_body_angular_velocity = 0.0f;

	if ( m_upper_body == nullptr )
	{
		upper_body_definition.type = b2_dynamicBody;
		upper_body_definition.position = position;
		upper_body_definition.linearDamping = 0.0f;
		upper_body_definition.linearVelocity.SetZero( );
		upper_body_definition.fixedRotation = true;
		upper_body_definition.allowSleep = true;

		lower_body_definition.type = b2_dynamicBody;
		lower_body_definition.position = position + b2Vec2( 0.0f, aabb_half_size.x );
		lower_body_definition.linearDamping = 0.0f;
		lower_body_definition.linearVelocity.SetZero( );
		lower_body_definition.angle = 0.0f;
		lower_body_definition.angularDamping = 0.1f;
		lower_body_definition.angularVelocity = 0.0f;
		lower_body_definition.allowSleep = true;
	}
	else
	{
		upper_body_definition.type = b2_dynamicBody;
		upper_body_definition.position = m_upper_body->GetPosition( );
		upper_body_definition.linearDamping = 0.0f;
		upper_body_definition.linearVelocity = m_upper_body->GetLinearVelocity( );
		upper_body_definition.fixedRotation = true;
		upper_body_definition.allowSleep = true;

		lower_body_definition.type = b2_dynamicBody;
		lower_body_definition.position = m_lower_body->GetPosition( );
		lower_body_definition.linearDamping = 0.0f;
		lower_body_definition.linearVelocity.SetZero( );// = m_upper_body->GetLinearVelocity( ); // m_lower_body->GetLinearVelocity( );
		lower_body_definition.angle = m_lower_body->GetAngle( );
		lower_body_definition.angularDamping = 0.1f;
		lower_body_definition.angularVelocity = 0.0f; //m_lower_body->GetAngularVelocity( );
		lower_body_definition.allowSleep = true;

		lower_body_linear_velocity = m_lower_body->GetLinearVelocity( );
		lower_body_angular_velocity = m_lower_body->GetAngularVelocity( );
	}

	m_upper_body.reset( );
	m_lower_body.reset( );

	m_upper_body = create_body( world, &upper_body_definition );
	m_lower_body = create_body( world, &lower_body_definition );

	m_upper_user_data.reset( new player_user_data( m_upper_body ) );
	m_upper_body->SetUserData( m_upper_user_data.get( ) );
	//m_lower_body->SetUserData( m_upper_user_data.get( ) );

	{
		b2PolygonShape upper_body_shape;
		upper_body_shape.SetAsBox( aabb_half_size.x, aabb_half_size.y, sprite_half_size, 0.0f );

		b2FixtureDef upper_body_fixture;
		upper_body_fixture.isSensor = false;
		upper_body_fixture.shape = &upper_body_shape;
		upper_body_fixture.friction = 1.0f;
		upper_body_fixture.restitution = 0.5f;
		upper_body_fixture.density = 10.0f;

		m_upper_body->CreateFixture( &upper_body_fixture );
	}

	{
		b2CircleShape lower_body_shape;
		lower_body_shape.m_p.Set( sprite_half_size.x, sprite_half_size.y + aabb_half_size.y - aabb_half_size.x );
		lower_body_shape.m_radius = aabb_half_size.x;

		b2FixtureDef lower_body_fixture;
		lower_body_fixture.isSensor = false;
		lower_body_fixture.shape = &lower_body_shape;
		lower_body_fixture.friction = 1.0f;
		lower_body_fixture.restitution = 0.01f;
		lower_body_fixture.density = 10.0f;

		m_lower_body->CreateFixture( &lower_body_fixture );
	}

	//OutputDebugStringA( "=====================\n" );
	//OutputDebugStringA( nmn::create_string( "Upper Linear Velocity: { %1%, %2% }\n", m_upper_body->GetLinearVelocity( ).x, m_upper_body->GetLinearVelocity( ).y ).c_str( ) );
	//OutputDebugStringA( nmn::create_string( "Lower Linear Velocity: { %1%, %2% }\n", m_lower_body->GetLinearVelocity( ).x, m_lower_body->GetLinearVelocity( ).y ).c_str( ) );

	revolute_joint_definition.Initialize( m_upper_body.get( ), m_lower_body.get( ), position + sprite_half_size + b2Vec2( 0.0f, aabb_half_size.y ) );
	revolute_joint_definition.collideConnected = false;
	revolute_joint_definition.enableMotor = true;
	revolute_joint_definition.maxMotorTorque = 40000.0f;
	revolute_joint_definition.motorSpeed = motor_speed;

	m_revolute = create_joint<b2RevoluteJoint>( world, &revolute_joint_definition );

	m_lower_body->SetAngularVelocity( lower_body_angular_velocity );
	m_lower_body->SetLinearVelocity( lower_body_linear_velocity );

	//OutputDebugStringA( "=====================\n" );
	//OutputDebugStringA( nmn::create_string( "Upper Linear Velocity: { %1%, %2% }\n", m_upper_body->GetLinearVelocity( ).x, m_upper_body->GetLinearVelocity( ).y ).c_str( ) );
	//OutputDebugStringA( nmn::create_string( "Lower Linear Velocity: { %1%, %2% }\n", m_lower_body->GetLinearVelocity( ).x, m_lower_body->GetLinearVelocity( ).y ).c_str( ) );
}

void player::switch_state( player_state::enum_ new_state )
{
	if ( m_state != new_state )
	{
		m_state = new_state;
		m_state_animations[m_state][m_direction]->reset( );

		switch ( m_state )
		{
		case player_state::jumping:
			{
				m_jump_held_for = 0.0f;
				m_falling = is_in_air( );

				if ( !m_falling )
				{
					m_jump_sound.Play( );
					float total_mass = m_lower_body->GetMass( ) + m_upper_body->GetMass( );
					m_upper_body->ApplyLinearImpulse( total_mass * b2Vec2( 0.0f, -10.0f * jump_power ), m_upper_body->GetPosition( ) );
				}
			}
			break;

		case player_state::dying:
			{
				m_death_sound.Play( );
				m_has_been_dead_for = 0.0f;
			}
			break;

		default:
			break;
		}
	}
}

void player::switch_direction( player_direction::enum_ new_direction )
{
	if ( m_direction != new_direction )
	{
		m_direction = new_direction;
		m_state_animations[m_state][m_direction]->reset( );
	}
}

void player::message_callback( std::string const & message_name, message_data_ptr data )
{
	if ( m_state != player_state::dying )
	{
		if ( message_name == collision_message::name( ) )
		{
			auto message = boost::shared_static_cast<collision_message const>( data );
			body_ptr other;

			if ( message->m_body_a.lock( ) == m_upper_body || message->m_body_a.lock( ) == m_lower_body )
			{
				other = message->m_body_b.lock( );
			}
			else if ( message->m_body_b.lock( ) == m_upper_body || message->m_body_b.lock( ) == m_lower_body )
			{
				other = message->m_body_a.lock( );
			}
			else
			{
				return; // Not me, I don't care.
			}

			if ( message->m_which == "thing" )
			{
				++m_things;
				m_pickup_sound.Play( );
				g_messenger.push( kill_object_message::name( ), message_data_ptr( new kill_object_message( other ) ) );
			}
			else if ( message->m_which == "insta_death" )
			{
				switch_state( player_state::dying );
			}
			else if ( message->m_which == "exit" )
			{
				g_messenger.push( level_over_message::name( ), message_data_ptr( new level_over_message( true ) ) );
			}
		}
	}
}

bool player::is_in_air( )
{
	b2Fixture * wheel_fixture = m_lower_body->GetFixtureList( );

	auto find_other = [&wheel_fixture]( b2Fixture* a, b2Fixture* b, b2Vec2 & normal )->b2Fixture *
	{
		if ( a == wheel_fixture )
		{
			normal *= -1.0f;
			return b;
		}
		else if ( b == wheel_fixture )
		{
			return a;
		}

		return nullptr;
	};

	for (	b2ContactEdge * contact_edge = m_lower_body->GetContactList( );
			contact_edge != nullptr;
			contact_edge = contact_edge->next )
	{
		b2Contact * contact = contact_edge->contact;

		if ( contact->GetManifold( )->pointCount != 0 )
		{
			b2WorldManifold world_manifold;
			contact->GetWorldManifold( &world_manifold );

			b2Vec2 normal = world_manifold.normal;
			b2Fixture * other = find_other( contact->GetFixtureA( ), contact->GetFixtureB( ), normal );

			if ( b2Dot( m_upper_body->GetLinearVelocity( ), normal ) < 0.001f )
			{
				if ( b2Dot( normal, b2Vec2( 0.0f, -1.0f ) ) >= 0.707f )
				{
					m_time_last_touched_floor = m_time;
					return false;
				}
			}
		}
	}

	return m_state == player_state::jumping || ( ( m_time - m_time_last_touched_floor ) > time_off_floor_before_fall );
}
